//
// Created by az on 2020/6/20.
//

#include "map_generator.h"

void MAP_GENERATOR::loadFile() {
    fp_.open(file_name_);
    if(!fp_){
        std::cerr << "open the map file is error ! " << std::endl;
        exit(-1);
    }

    free_cell_ = sdf_tools::COLLISION_CELL(-127);
    obst_cell_ = sdf_tools::COLLISION_CELL( 128);

    builtGlobalMap();
    fp_.close();
}

void MAP_GENERATOR::builtGlobalMap() {
//    std::vector<std::pair<double, double>> coord;
    std::vector<std::pair<int, int>> coord;
    std::string line;
    double xmin = 0, xmax = 0, ymin = 0, ymax = 0;
//    double xtmp, ytmp;
    int xtmp, ytmp;
    //read coordinates from file
    bool is_file_mepty = true;
    file_lock_.lock();
    while (getline(fp_, line)){
        if(is_file_mepty){is_file_mepty = false;}

        if(line[0] == '$'){

            int index = line.find(',');
            if(line.substr(0, index) == "$obstacle"){
                auto s1 = line.substr(index+1, line.size()-index-1);
                index = s1.find(',');

                xtmp  = atoi(s1.substr(0, index).c_str());

                auto s2 = s1.substr(index+1, s1.size()-index-1);
                index  = s2.find(',');
                ytmp   = atoi(s2.substr(0, index).c_str());

//                if(std::isinf(xtmp) || std::isinf(ytmp) || std::isnan(xtmp) || std::isnan(ytmp))
//                    continue;

                coord.emplace_back(std::make_pair(xtmp, ytmp));
//                xmin = xtmp < xmin ? xtmp : xmin;
//                xmax = xtmp > xmax ? xtmp : xmax;
//                ymin = ytmp < ymin ? ytmp : ymin;
//                ymax = ytmp > ymax ? ytmp : ymax;
            }
            else if(line.substr(0, index) == "$resolution") {
                resolution_ = atof(line.substr(index+1, line.size()-index-1).c_str());
            }
            else if(line.substr(0, index) == "$mapsize") {
                line = line.substr(index+1, line.size()-index-1);
                index = line.find(',');
                xmin  = atof(line.substr(0, index).c_str());

                line = line.substr(index+1, line.size()-index-1);
                index  = line.find(',');
                xmax   = atof(line.substr(0, index).c_str());

                line = line.substr(index+1, line.size()-index-1);
                index  = line.find(',');
                ymin   = atof(line.substr(0, index).c_str());

                ymax   = atof(line.substr(index+1, line.size()-index-1).c_str());
            }

        }
    }
    file_lock_.unlock();

    if(!is_file_mepty) {
        //map size
        if(xmax - xmin < 20) { xmax += 10; xmin -= 10;}
        if(ymax - ymin < 20) { ymax += 10; ymin -= 10;}

        double xsize = xmax - xmin + 2 * map_margin_;
        std::cout << "xmax : " << xmax << std::endl;
        double ysize = ymax - ymin + 2 * map_margin_;
        std::cout << "ymax : " << ymax << std::endl;
        double zsize = resolution_;

        map_origin_ << xmin - map_margin_, ymin - map_margin_, 0;

        Eigen::Translation3d origin_translation(map_origin_(0), map_origin_(1), map_origin_(2));
        Eigen::Quaterniond origin_rotation(1.0, 0.0, 0.0, 0.0);
        Eigen::Affine3d origin_transform = origin_translation * origin_rotation;
        collision_map_.reset(new sdf_tools::CollisionMapGrid(origin_transform, "world", resolution_, xsize, ysize, zsize, free_cell_));

        int64_t index_x, index_y, index_z;
        for (size_t i = 0; i < coord.size(); ++i) {
//            Eigen::Vector3d add_pt(coord[i].first, coord[i].second, 0);
//            collision_map_->Set3d(add_pt, obst_cell_);
            index_x = coord[i].first;
            index_y = coord[i].second;
            index_z = 0;
            collision_map_->Set(index_x, index_y, index_z, obst_cell_);
        }

        x_min_ = xmin - map_margin_;
        x_max_ = xmax + map_margin_;
        y_min_ = ymin - map_margin_;
        y_max_ = ymax + map_margin_;
    }
    else { // the file has no data !
        std::cout << "[map generator] : the file " << file_name_ << " has no map data !" << std::endl;
        double xsize = 100 + 2 * map_margin_;
        double ysize = 100 + 2 * map_margin_;
        double zsize = resolution_;
        map_origin_ << -xsize/2, -ysize/2, 0;

        Eigen::Translation3d origin_translation(map_origin_(0), map_origin_(1), map_origin_(2));
        Eigen::Quaterniond origin_rotation(1.0, 0.0, 0.0, 0.0);
        Eigen::Affine3d origin_transform = origin_translation * origin_rotation;
        collision_map_.reset(new sdf_tools::CollisionMapGrid(origin_transform, "world", resolution_, xsize, ysize, zsize, free_cell_));

        x_min_ = -xsize/2;
        x_max_ =  xsize/2;
        y_min_ = -ysize/2;
        y_max_ =  ysize/2;
    }

    has_map_ = true;
    pointInflate();
}

void MAP_GENERATOR::reMappingGMap(Eigen::Vector3d pt){
    //write collision_map_'s data to file
    writeFile();

    if(!has_map_ || !is_mapping_) return;

    fp_.open(file_name_);
    const double map_size_inflate = 10;
    bool xmin_change = false, ymin_change = false;
    if(pt(0) < x_min_) {
        xmin_change = true;
        x_min_ -= map_size_inflate;
    }
    if(pt(1) < y_min_) {
        ymin_change = true;
        y_min_ -= map_size_inflate;
    }
    x_max_ = (x_max_ > pt(0)) ? x_max_ : (pt(0) + map_size_inflate);
    y_max_ = (y_max_ > pt(1)) ? y_max_ : (pt(1) + map_size_inflate);

    double xsize = (x_max_ - x_min_);
    double ysize = (y_max_ - y_min_);
    double zsize = resolution_;
    map_origin_ << x_min_, y_min_, 0;

    Eigen::Translation3d origin_translation(map_origin_(0), map_origin_(1), map_origin_(2));
    Eigen::Quaterniond origin_rotation(1.0, 0.0, 0.0, 0.0);
    Eigen::Affine3d origin_transform = origin_translation * origin_rotation;
    file_lock_.lock();
    collision_map_.reset(new sdf_tools::CollisionMapGrid(origin_transform, "world", resolution_, xsize, ysize, zsize, free_cell_));

    sdf_tools::COLLISION_CELL odds;
    std::cout << "[map generator] : re mapping ! " << std::endl;
    std::string line;
    int xtmp = 0, ytmp = 0;
    double oddtmp = 0;
    int64_t index_x, index_y, index_z;
    while (getline(fp_, line)){

        if(line[0] == '$'){
            int index = line.find(',');
            if(line.substr(0, index) == "$obstacle"){

                auto s1 = line.substr(index + 1, line.size() - index - 1);
                index = s1.find(',');

                xtmp = atoi(s1.substr(0, index).c_str());

                auto s2 = s1.substr(index + 1, s1.size() - index - 1);
                index = s2.find(',');
                ytmp = atoi(s2.substr(0, index).c_str());

                oddtmp = atof(s2.substr(index+1, s2.size()-index-1).c_str());

//                index_x = xtmp;
//                index_y = ytmp;
                index_x = xtmp + (xmin_change ? (map_size_inflate * inv_resolution_) : 0);
                index_y = ytmp + (ymin_change ? (map_size_inflate * inv_resolution_) : 0);
                index_z = 0;
                odds.occupancy = (float)oddtmp;
                collision_map_->Set(index_x, index_y, index_z, odds);
//                Eigen::Vector3d add_pt(xtmp, ytmp, 0);
//                odds.occupancy = oddtmp;
//                collision_map_->Set3d(add_pt, odds);
            }
        }
    }
    file_lock_.unlock();

    fp_.close();
//    pointInflate();
}

std::vector<double> MAP_GENERATOR::getMapSize() {
    std::vector<double> re_size;
    if(!has_map_) return re_size;

    re_size.push_back(x_min_);
    re_size.push_back(x_max_);
    re_size.push_back(y_min_);
    re_size.push_back(y_max_);
//    std::cout << x_min_ << "," << x_max_ << "," << y_min_ << "," << y_max_ << "\n"
//    << re_size[0] << "," << re_size[1] << "," << re_size[2] << "," << re_size[3] << std::endl;
    return re_size;
}
/*占据栅格膨胀处理*/
void MAP_GENERATOR::pointInflate() {
    int64_t n_inflate = ceil(inflate_size_ * inv_resolution_);
    std::vector<std::pair<int64_t , int64_t >> v_obs;

    auto isLocalInside = [&](const Eigen::Vector3d& loc){
        const auto& index = collision_map_local_->LocationToGridIndex(loc);
        return collision_map_local_->Inside(index);
    };

    for (int64_t x = 0; x < collision_map_->GetNumXCells(); x++)
        for (int64_t y = 0; y < collision_map_->GetNumYCells(); y++){
            const auto& loc = collision_map_->GridIndexToLocation(Eigen::Vector3i(x, y, 0));
            if(!is_mapping_ && has_local_map_ && isLocalInside(loc)){
                const auto& local_cell = collision_map_local_->Get(loc[0], loc[1], loc[2]);
                if(local_cell.second && local_cell.first.occupancy > 0.5)
                    v_obs.emplace_back(std::make_pair(x, y));
            } else {
                const auto& global_cell = collision_map_->Get(x, y, 0);
                if(global_cell.first.occupancy > 0.5)
                    v_obs.emplace_back(std::make_pair(x, y));
            }
        }
    //mapping inflate map
    map_lock_.lock();
    double xsize = (x_max_ - x_min_);
    double ysize = (y_max_ - y_min_);
    double zsize = resolution_;
    Eigen::Translation3d origin_translation(map_origin_(0), map_origin_(1), map_origin_(2));
    Eigen::Quaterniond origin_rotation(1.0, 0.0, 0.0, 0.0);
    Eigen::Affine3d origin_transform = origin_translation * origin_rotation;
    inflate_map_.reset(new sdf_tools::CollisionMapGrid(origin_transform, "world", resolution_, xsize, ysize, zsize, free_cell_));

    auto iter = v_obs.begin();
    for (; iter != v_obs.end(); ++iter) {
        auto point = *iter;
        for (int64_t nx = -n_inflate; nx < n_inflate; ++nx)
            for (int64_t ny = -n_inflate; ny < n_inflate; ++ny)
                if(pow(nx, 2) + pow(ny, 2) <= pow(n_inflate, 2))
                    inflate_map_->Set(point.first + nx, point.second + ny, 0, obst_cell_);
    }

//    if(!is_mapping_ && has_local_map_){
//        for(int64_t lx = 0; lx < collision_map_local_->GetNumXCells(); lx++)
//            for(int64_t ly = 0; ly < collision_map_local_->GetNumYCells(); ly++){
//                const auto& loc = collision_map_local_->GridIndexToLocation(Eigen::Vector3i(lx, ly, 0));
//                const auto& local_cell = collision_map_local_->Get(loc[0], loc[1], loc[2]);
//                if(local_cell.second )
//                    inflate_map_->Set(loc[0], loc[1], loc[2], local_cell.first.occupancy);
//            }
//    }

//    inflate_map_->Set(robot_pt_[0], robot_pt_[1], robot_pt_[2], free_cell_);
    map_lock_.unlock();
}

std::vector<int64_t> MAP_GENERATOR::location2Index(const Eigen::Vector3d& location, PLANSTYLE style) {
    std::vector<int64_t> grid_index;
    if (style == GLOBAL){
        if(!has_map_)
            return grid_index;
        grid_index = collision_map_->LocationToGridIndex3d(location);
    }

    return grid_index;
}

bool MAP_GENERATOR::getGlobalMap(shared_ptr<sdf_tools::CollisionMapGrid>& _map) {
    if(!has_map_) return false;

    _map = this->inflate_map_;
//    _map = this->collision_map_;
    return true;
}

bool MAP_GENERATOR::getLocalMap(shared_ptr<sdf_tools::CollisionMapGrid>& _map) {
    if(!has_map_) return false;

    _map = this->collision_map_local_;
    return true;
}

/******  构建局部地图  ******/
void MAP_GENERATOR::builtLocalMap(const Eigen::Vector3d& start_pt) {
    if(start_pt[0] < x_min_ || start_pt[0] > x_max_
    || start_pt[1] < y_min_ || start_pt[1] > y_max_){
        has_local_map_ = false;
        return;
    }

    robot_pt_ = start_pt;

    shared_ptr<sdf_tools::CollisionMapGrid> local_base_map;
//    local_base_map = inflate_map_;
    local_base_map = is_mapping_ ? inflate_map_ : collision_map_;

    auto index_in_map  = this->collision_map_->LocationToGridIndex(robot_pt_);
    auto pt_from_index = this->collision_map_->GridIndexToLocation(index_in_map);
    double local_c_x = pt_from_index(0) - x_local_size_/2.0;
    double local_c_y = pt_from_index(1) - y_local_size_/2.0;

    local_origin_ << local_c_x, local_c_y, 0;

    Eigen::Translation3d origin_local_translation( local_origin_(0), local_origin_(1), local_origin_(2));
    Eigen::Quaterniond origin_local_rotation(1.0, 0.0, 0.0, 0.0);
    Eigen::Affine3d origin_local_transform = origin_local_translation * origin_local_rotation;

    double z_local_size = resolution_;
    map_lock_.lock();
    collision_map_local_.reset(new sdf_tools::CollisionMapGrid(origin_local_transform, "world", resolution_, x_local_size_, y_local_size_, z_local_size, free_cell_));

    auto point_local_begin = collision_map_local_->GridIndexToLocation(0, 0, 0);
    double &px = point_local_begin[0];
    double &py = point_local_begin[1];
    double &pz = point_local_begin[2];
    const auto& xsize = collision_map_local_->GetNumXCells();
    const auto& ysize = collision_map_local_->GetNumYCells();
    const auto& zsize = collision_map_local_->GetNumZCells();
    if(px <= this->x_min_ || py <= this->y_min_){
        auto gxmin = px <= x_min_ ? x_min_ : px;
        auto gymin = py <= y_min_ ? y_min_ : py;
        auto index_local_start = collision_map_local_->LocationToGridIndex(gxmin, gymin, 0);
        const auto& lx = index_local_start[0];
        const auto& ly = index_local_start[1];
        const auto& lz = index_local_start[2];
        auto index_global = collision_map_->LocationToGridIndex(gxmin, gymin, 0);
        const int64_t begin_x = index_global[0] - lx;
        const int64_t begin_y = index_global[1] - ly;
        for(int64_t x = lx; x < xsize; x++)
            for(int64_t y = ly; y < ysize; y++)
                for(int64_t z = lz; z < zsize; z++){
                    auto cell = local_base_map->Get(x+begin_x, y+begin_y, z);
                    if(cell.second && cell.first.occupancy > 0.5){
                        collision_map_local_->Set(x, y, z, obst_cell_);
                    }
                }
    } else {
        auto point_map_global  = collision_map_->LocationToGridIndex(px, py, pz);
        const auto& gx = point_map_global[0];
        const auto& gy = point_map_global[1];
        const auto& gz = point_map_global[2];
        for(int64_t x = 0; x < xsize; x++)
            for(int64_t y = 0; y < ysize; y++)
                for(int64_t z = 0; z < zsize; z++){
                    auto cell = local_base_map->Get(gx+x, gy+y, gz+z);
                    if(cell.second && cell.first.occupancy > 0.5){
                        collision_map_local_->Set(x, y, z, obst_cell_);
                    }
                }
    }

    has_local_map_ = true;
    map_lock_.unlock();
}

void MAP_GENERATOR::getMapOrigin(Eigen::Vector3d& _origin) {
    if (!has_map_) {
        std::cerr << "[map generator :] get map origin false !" << std::endl;
        exit(-1);
    }

    _origin = this->map_origin_;
}

void MAP_GENERATOR::getLocalOrigin(Eigen::Vector3d& _origin){
    if (!has_map_) {
        std::cerr << "[map generator :] get map origin false !" << std::endl;
        exit(-1);
    }

    _origin = this->local_origin_;
}

//(_x, _y)为机器人坐标
void MAP_GENERATOR::refreshMap(double _x, double _y, std::vector<std::pair<Eigen::Vector3d, bool >>& _vec_point) {
    if(_x < x_min_ || _x > x_max_ || _y < y_min_ || _y > y_max_) return;

    std::shared_ptr<sdf_tools::CollisionMapGrid> _map;
    _map = is_mapping_ ? collision_map_ : collision_map_local_;

    auto index_rob = _map->LocationToGridIndex(_x, _y, 0.1);
    vec_pt_.swap(_vec_point);
    auto iter = vec_pt_.begin();
    sdf_tools::COLLISION_CELL odds;
    map_lock_.lock();
    for(; iter != vec_pt_.end(); ++iter){
        auto& pt = iter->first;
        auto cell = _map->Get(pt[0], pt[1], pt[2]);
        if(!iter->second){
            if(is_mapping_) continue;
            //处理局部地图内或外的地面信息
            auto index_ground = getGroundIndex(cell.second, pt, _x ,_y);
            odds.occupancy = oddsmin;
            _map->Set(index_ground[0], index_ground[1], index_ground[2], odds);
            bresenHamLine(_map, index_rob[0], index_rob[1], index_ground[0], index_ground[1]);
        }
        else if(cell.second){ //处理局部地图内或外的障碍物信息
            if(abs(pt[0] - _x) > 0.5*x_local_size_ || abs(pt[1] - _y) > 0.5*x_local_size_)
                continue;
            //get the occupied state of cell : log p(o)/p(f)
            float& value = cell.first.occupancy;

            value += (iter->second ? looccu : lofree);
            value = (value > oddsmax) ? oddsmax : value;
            value = (value < oddsmin) ? oddsmin : value;
            odds.occupancy = value;
            auto index_obs = _map->LocationToGridIndex(pt[0], pt[1], 0.1);
            _map->Set(index_obs[0], index_obs[1], index_obs[2], odds);
            if(is_mapping_) //记录地图或无地面点云信息时启用
                bresenHamLine(_map, index_rob[0], index_rob[1], index_obs[0], index_obs[1]);
        }
        else {
            //探测数据超出collision_map_范围
            auto outside_map = [&](){
                return !collision_map_->Get3d(pt).second;
            };

            if(is_mapping_ && outside_map()) {
                reMappingGMap(pt);
                break;
            }
        }
    }
//    _map->Set(index_rob[0], index_rob[1], index_rob[2], free_cell_);
    map_lock_.unlock();

    pointInflate();
}

std::vector<int64_t> MAP_GENERATOR::getGroundIndex(bool mark, Eigen::Vector3d &pt, const double& _x, const double& _y) {
    std::shared_ptr<sdf_tools::CollisionMapGrid> _map;
    _map = is_mapping_ ? collision_map_ : collision_map_local_;

    if(mark) {
        return _map->LocationToGridIndex(pt[0], pt[1], 0.1);
    }

    if(pt[0] == _x){
        pt[1] = pt[1] > _y ? (_y + 0.5*x_local_size_ - 0.5*resolution_) : (_y - 0.5*x_local_size_ + 0.5*resolution_);
    } else {
        double gradient = (pt[1] - _y) / (pt[0] - _x);
        if(gradient >= -1 && gradient <= 1){
            double _k = (0.5*x_local_size_ - 0.5*resolution_) / abs(pt[0] - _x);
            pt[0] = pt[0] > _x ? (_x + 0.5*x_local_size_ - 0.5*resolution_) : (_x - 0.5*x_local_size_ + 0.5*resolution_);
            pt[1] = pt[1] > _y ? (_y + _k * abs(pt[1] - _y)) : (_y - _k * abs(pt[1] - _y));
        } else {
            double _k = (0.5*x_local_size_ - 0.5*resolution_) / abs(pt[1] - _y);
            pt[1] = pt[1] > _y ? (_y + 0.5*x_local_size_ - 0.5*resolution_) : (_y - 0.5*x_local_size_ + 0.5*resolution_);
            pt[0] = pt[0] > _x ? (_x + _k * abs(pt[0] - _x)) : (_x - _k * abs(pt[0] - _x));
        }
    }

    return _map->LocationToGridIndex(pt[0], pt[1], 0.1);
}

void MAP_GENERATOR::bresenHamLine(shared_ptr<sdf_tools::CollisionMapGrid>& _map, int64_t x0, int64_t y0, int64_t& x1, int64_t& y1) {
    int64_t dx = abs(x1 - x0), sx = x0 < x1 ? 1 : -1;
    int64_t dy = abs(y1 - y0), sy = y0 < y1 ? 1 : -1;
    int64_t err = (dx > dy? dx : -dy)/2, e2;

    sdf_tools::COLLISION_CELL odds;
    for(;;){
        if(x0==x1 && y0 == y1) break;
        //
        auto cell = _map->Get((long)x0, (long)y0, 0);
        float value = cell.first.occupancy + lofree;

        value = (value > oddsmax) ? oddsmax : value;
        value = (value < oddsmin) ? oddsmin : value;
        odds.occupancy = is_mapping_ ? value : oddsmin;

        _map->Set(x0, y0, 0, odds);

        e2 = err;
        if(e2 > -dx) { err -= dy; x0 += sx;}
        if(e2 <  dy) { err += dx; y0 += sy;}
    }
}

void MAP_GENERATOR::writeFile() {
    file_lock_.lock();
    wf_.open(file_name_);
    if(!wf_){
        std::cerr << "open the map file is error ! " << std::endl;
        exit(-1);
    }

    wf_ << "$resolution," << resolution_ << endl;
    wf_ << "$original_point,30.528628340918,114.35572874791,22.813257" << endl;
    wf_ << "$mapsize," << x_min_ + map_margin_ << ","
    << x_max_ - map_margin_ << ","
    << y_min_ + map_margin_ << ","
    << y_max_ - map_margin_ << std::endl;
//    Eigen::Vector3i index;
    for (int64_t x = 0; x < collision_map_->GetNumXCells(); x++)
        for (int64_t y = 0; y < collision_map_->GetNumYCells(); y++){
            auto cell = collision_map_->Get(x, y, 0);
            if (cell.first.occupancy > 0.5) {
//                index << x, y, 0;
//                auto obs = collision_map_->GridIndexToLocation(index);
//                wf_ << "$obstacle," << obs[0] << "," << obs[1]
//                << "," << cell.first.occupancy << std::endl;
                wf_ << "$obstacle," << x << "," << y
                    << "," << cell.first.occupancy << std::endl;
            }
        }

    wf_.close();
    file_lock_.unlock();

    std::cout << "rewrite map file ! " << std::endl;
}

bool MAP_GENERATOR::checkCoordObs(Eigen::Vector3d checkPt)
{
    if(inflate_map_->Get(checkPt(0), checkPt(1), checkPt(2)).first.occupancy > 0.0 )
        return true;

    return false;
}